
for i = 1:n
    qi = Q(i, :);
    x_real(3*i-2:3*i, 1) = robot_real.fkine(qi).t;
end

alpha = 0.01;
beta = 0.9;

iter_max = 20;
finish = false;
para_k = zeros(1, 12);

for k = 1:iter_max
    
    for i = 1:n
        qi = Q(i, :);
        M(3*i-2:3*i, :) = calculate_Jacobian(para_k, DH_Para, qi);
    end

    error_x = calculate_error(para_k, DH_Para, x_real, Q);
    delta_para = pinv(M) * error_x;
    
    tk = 1;
    while norm(calculate_error(para_k + tk * delta_para', DH_Para, x_real, Q)) > norm(error_x)
        tk = beta * tk;
        if tk < 0.1
            finish = true;
            delta_para = 0 * delta_para;
            break
        end
    end
    
    para_k = para_k + tk * delta_para';
    if finish
        break;
    end
end

robot.offset = robot.offset + para_k(1:6);
robot.links(1).d = robot.links(1).d + para_k(7);
robot.links(3).a = robot.links(3).a + para_k(8);
robot.links(4).a = robot.links(4).a + para_k(9);
robot.links(4).d = robot.links(4).d + para_k(10);
robot.links(5).d = robot.links(5).d + para_k(11);
robot.links(6).d = robot.links(6).d + para_k(12);

disp("zero offset: ");
disp(para_k(1:6));
disp("link offset: ");
disp(para_k(7:12));